// #include <Ticker.h>
#include "motor.h"
#include "pid.h"




void setup() {
  Serial.begin(115200);

	pinMode(ENCODER_L, INPUT);       //编码器引脚 输入模式
	pinMode(ENCODER_R, INPUT);       //编码器引脚 输入模式
	pinMode(DIRECTION_L, INPUT);     //编码器引脚 输入模式
	pinMode(DIRECTION_R, INPUT);     //编码器引脚 输入模式
	
	//编码器接口1 开启外部跳边沿中断 
	attachInterrupt(ENCODER_L, READ_ENCODER_L, CHANGE);  //中断函数READ_ENCODER_L
	//编码器接口2 开启外部跳边沿中断 
	attachInterrupt(ENCODER_R, READ_ENCODER_R, CHANGE);  //中断函数READ_ENCODER_R
	
	interrupts();                      //打开外部中断  
  timer_read_encoder.attach_ms(interrupt_time, read);  

  motor_pinint();        //电机引脚初始化

  init_pid();

  gv_pid_speed_set_r = 60;
  gv_pid_speed_set_l = 60;
}

void loop() {
  read();
  // motorRun_forward();
  check_pid();
  set_speed_l(gv_pid_speed_out_r);
  // delay(500);
  // motorRun_turnright();
  // delay(500);
  // motorRun_turnleft();
  // delay(500);
  // motorStop();
  // delay(500);

}


/**************************************************************************
函数功能：外部中断读取编码器数据，具有二倍频功能 注意外部中断是跳变沿触发
入口参数：无
返回  值：无
**************************************************************************/
void READ_ENCODER_L(void) {
  if (digitalRead(ENCODER_L) == LOW) {     //如果是下降沿触发的中断
    if (digitalRead(DIRECTION_L) == LOW)      Velocity_L--;  //根据另外一相电平判定方向
    else      Velocity_L++;
  }
  else {     //如果是上升沿触发的中断
    if (digitalRead(DIRECTION_L) == LOW)      Velocity_L++; //根据另外一相电平判定方向
    else     Velocity_L--; 
  }
}
/**************************************************************************
函数功能：外部中断读取编码器数据，具有二倍频功能 注意外部中断是跳变沿触发
入口参数：无
返回  值：无
**************************************************************************/
void READ_ENCODER_R(void) {
  if (digitalRead(ENCODER_R) == LOW) { //如果是下降沿触发的中断
    if (digitalRead(DIRECTION_R) == LOW)      Velocity_R--;//根据另外一相电平判定方向
    else      Velocity_R++;
  }
  else {   //如果是上升沿触发的中断
    if (digitalRead(DIRECTION_R) == LOW)      Velocity_R++; //根据另外一相电平判定方向
    else     Velocity_R--;
  }
}

void read(void)
{
/**************************************************************************
计算为转速n/s  500线光电编码器数据 定时时间为20MS
 转速 = num/500/20ms
 转速 = num*2/time
**************************************************************************/
  Velocity_Left = Velocity_L*21/interrupt_time;    Velocity_L = 0;  //读取左轮编码器数据，并清零，这就是通过M法测速（单位时间内的脉冲数）得到速度。
  Velocity_Right = Velocity_R*20/interrupt_time;    Velocity_R = 0; //读取右轮编码器数据，并清零  （条变沿采样）*3.14*0.065（直径）/（11（单圈总脉冲）*20/1000）
  //  Serial.print("Velocity_Left = \t");
  //  Serial.println(Velocity_Left);
  //  Serial.print("Velocity_Right = \t");
  //  Serial.println(Velocity_Right);
  Serial.print("Velocity_L:");Serial.print(Velocity_L);
	Serial.print(" | Velocity_Left:");Serial.println(Velocity_Left);
	Serial.print("Velocity_R:");Serial.print(Velocity_R);
	Serial.print(" | Velocity_Right:");Serial.println(Velocity_Right);

  // if ( myPID_r.Compute()) {
  //   int lv_speed = gv_pid_speed_out_r;
  //   set_speed_r( lv_speed, m_forward );
  //   Serial.println(gv_pid_speed_set_r);
  //   Serial.println(gv_pid_speed_r);
  //   Serial.println(gv_pid_speed_out_r);
  //   Serial.println(lv_speed);
  // }

}
